close all;
clear all;
%***************************系统参数**************************************
Jx = 5.455e-3;  Jy = 5.455e-3;  Jz = 1.089e-2;
beta0 = 1.5;    beta1 = 6.0;
k1 = 5.0;   k2 = 1.0;
%***************************系统初值**************************************
x0 = [0.05; -0.25; 0.05; -0.25; 0.05; -0.25; 0; 0; 0; -4; -4; -4; 0; 0; 0; 0; 0; 0; 0; 0; 0;];

[t,x] = ode45('plant', [0 10], x0);

e_phi       = x(:,1);
de_phi      = x(:,2);
e_theta     = x(:,3);
de_theta    = x(:,4);
e_psi       = x(:,5);
de_psi      = x(:,6);
int_phi     = x(:,7);   
int_theta   = x(:,8);    
int_psi     = x(:,9);
g_phi       = x(:,10);  
g_theta     = x(:,11);   
g_psi       = x(:,12);
z0_phi = x(:,13);     z1_phi = x(:,14);     z2_phi = x(:,15);  
z0_theta = x(:,16);   z1_theta = x(:,17);   z2_theta = x(:,18);
z0_psi = x(:,19);     z1_psi = x(:,20);     z2_psi = x(:,21);
%***************************期望轨迹**************************************
phi_d   = -0.05+0.2*sin(2*pi/5*t);
theta_d = -0.05+0.2*sin(2*pi/5*t);
psi_d   = -0.05+0.2*sin(2*pi/5*t);

dphi_d   = 2*pi/5*0.2*cos(2*pi/5*t);
dtheta_d = 2*pi/5*0.2*cos(2*pi/5*t);
dpsi_d   = 2*pi/5*0.2*cos(2*pi/5*t);

ddphi_d   = -2*pi/5*2*pi/5*0.2*sin(2*pi/5*t);
ddtheta_d = -2*pi/5*2*pi/5*0.2*sin(2*pi/5*t);
ddpsi_d   = -2*pi/5*2*pi/5*0.2*sin(2*pi/5*t);

%***************************系统干扰**************************************
d_phi   = -Jx*(1+sin(0.2*pi*t));
d_theta = -Jy*(1+sin(0.2+0.3*pi*t));
d_psi   = -Jz*(1+sin(0.5+0.2*pi*t));
%***************************滑模面****************************************
sphi    = de_phi    + int_phi;
stheta  = de_theta  + int_theta;
spsi    = de_psi    + int_psi;
%***************************欧拉角****************************************
dphi = de_phi + dphi_d;
dtheta = de_theta + dtheta_d;
dpsi = de_psi + dpsi_d;
%***************************控制输入**************************************
tao_phi     = Jx * ( -(Jy-Jz)/Jx * dtheta .* dpsi + ddphi_d  - beta1*( sig(de_phi  ,2/3) + beta0*sig(e_phi  ,1/2) ) - k1*sig(sphi  ,1/2)-k2*sphi  +g_phi );
tao_theta   = Jy * ( -(Jz-Jx)/Jy * dphi   .* dpsi + ddtheta_d- beta1*( sig(de_theta,2/3) + beta0*sig(e_theta,1/2) ) - k1*sig(stheta,1/2)-k2*stheta+g_theta );
tao_psi     = Jz * ( -(Jx-Jy)/Jz * dtheta .* dphi + ddpsi_d  - beta1*( sig(de_psi  ,2/3) + beta0*sig(e_psi  ,1/2) ) - k1*sig(spsi  ,1/2)-k2*spsi  +g_psi);

%***************************画图****************************************
figure(1);
subplot(311);
plot(t,e_phi,'k','linewidth',2);
xlabel('Time/s');ylabel('phi');
legend('e_phi');
subplot(312);
plot(t,e_theta,'k','linewidth',2);
xlabel('Time/s');ylabel('theta');
legend('e_theta');
subplot(313);
plot(t,e_psi,'k','linewidth',2);
xlabel('Time/s');ylabel('psi');
legend('e_psi');


figure(2);
subplot(311);
plot(t,phi_d,'b',t,e_phi+phi_d,'r--','linewidth',2);
xlabel('Time/s');ylabel('phi');
legend('phi_d','phi tracking');
subplot(312);
plot(t,theta_d,'b',t,e_theta+theta_d,'r--','linewidth',2);
xlabel('Time/s');ylabel('theta');
legend('theta_d','theta tracking');
subplot(313);
plot(t,psi_d,'b',t,e_psi+psi_d,'r--','linewidth',2);
xlabel('Time/s');ylabel('psi');
legend('psi_d','psi tracking');


figure(3);
subplot(311);
plot(t,tao_phi,'k','linewidth',2);
xlabel('Time/s');ylabel('tao');
legend('tao_phi');
subplot(312);
plot(t,tao_theta,'k','linewidth',2);
xlabel('Time/s');ylabel('tao');
legend('tao_theta');
subplot(313);
plot(t,tao_psi,'k','linewidth',2);
xlabel('Time/s');ylabel('tao');
legend('tao_psi');


figure(4);
subplot(311);
plot(t,1/Jx*d_phi,'b',t,z1_phi,'r--','linewidth',2);
xlabel('Time/s');ylabel('phi');
legend('1/Jx*phi','rou_phi estimation');
subplot(312);
plot(t,1/Jy*d_theta,'b',t,z1_theta,'r--','linewidth',2);
xlabel('Time/s');ylabel('theta');
legend('1/Jy*theta','rou_theta estimation');
subplot(313);
plot(t,1/Jz*d_psi,'b',t,z1_psi,'r--','linewidth',2);
xlabel('Time/s');ylabel('psi');
legend('1/Jz*psi','rou_psi estimation');

